/**
 * @file servo_control.c
 * @brief SG90舵机控制实现
 * @date 2024-07-15
 *
 * @details
 * 实现SG90舵机控制的基本功能
 * 使用PWM4控制舵机角度
 */

#include "servo_control.h"

// 全局变量
static pwm_state_t g_servoState;
static pwm_config_t g_servoConfig;

/**
 * @brief 将角度转换为PWM计数值
 * @param angle 舵机角度(0-180度)
 * @return 对应的PWM计数值
 */
uint32_t angle_to_ticks(int angle) {
    if (angle < 0) angle = 0;
    if (angle > 180) angle = 180;
    return SERVO_MIN_PULSE_TICKS + (SERVO_MAX_PULSE_TICKS - SERVO_MIN_PULSE_TICKS) * angle / 180;
}

/**
 * @brief 设置舵机角度
 * @param angle 舵机角度(0-180度)
 */
void servo_set_angle(int angle) {
    g_servoConfig.duty = angle_to_ticks(angle);
    PWM_DRV_Cfg(SERVO_PWM_INST, &g_servoState, &g_servoConfig);
}

/**
 * @brief 初始化舵机
 */
void servo_init(void) {
    g_servoConfig.period = SERVO_PERIOD_TICKS;
    g_servoConfig.duty = angle_to_ticks(90); // 默认中位
    g_servoConfig.pwm_clk_src_cfg = G_CLK;
    g_servoConfig.pwm_polarity = S_RV_C_CMP;
    g_servoConfig.pwm_active_cfg = 0x1;
    g_servoConfig.prescaler = 0x4;
    g_servoConfig.repeat = ONCE;
    g_servoConfig.water_mark = 4;
    g_servoConfig.int_type = 0x7;
    g_servoConfig.callback = NULL;
    g_servoConfig.callbackParam = NULL;
    PWM_DRV_Init(SERVO_PWM_INST, &g_servoState, &g_servoConfig);
    PWM_DRV_Start(SERVO_PWM_INST);
}

/**
 * @brief 简单延时函数
 * @param ms 延时时间(毫秒)
 */
void delay_ms(uint32_t ms) {
    for (volatile uint32_t i = 0; i < ms * 8000; ++i) {
        __asm__("nop");
    }
} 